import time

import serial
import serial.tools.list_ports

ACCData=[0.0]*58           #52
FrameState = 0            #通过值判断属于哪一种情况
Bytenum = 0               #读取到这一段的第几位

a = [0.0]*3
w = [0.0]*3
m = [0.0]*3
Angle = [0.0]*3
kz = [0.0]*3    #输入的控制变量1,2

def DueData(inputdata):   #读取的数据到数组里
    global  FrameState    #在局部修改全局变量，要进行global的定义
    global  Bytenum
    global  a    #陀螺仪
    global  w    #加速度
    global  m    #地磁计
    global  Angle
    global kz

    for data in inputdata:  #输入的数据进行遍历
        if FrameState==0:   #当未确定状态的时候，进入以下判断
            if data==0x50 : #0x55位于第一位时候，开始读取数据，增大bytenum
                Bytenum=0
                FrameState=1
                continue

        elif FrameState==1: # acc    #已确定数据
            
            if Bytenum<57:            # 读取51个数据    51
                ACCData[Bytenum+1]=data # 从0开始
                Bytenum+=1
            else:
               if (ACCData[49] * 256.0 * 256.0 + ACCData[50] * 256.0 + ACCData[51] - 1000000) * 0.001 == 128 : #校验
                a = get_gyro(ACCData)   #陀螺仪
                w = get_acc (ACCData)   #加速度
                m = get_mg(ACCData)    #地磁计

                Angle = get_angle(ACCData)  #角度
                kz=get_kz(ACCData)  #输入的控制变量

                #各数据归零，进行新的循环判断
                #Bytenum=0
                #FrameState=0

                d = a+w+m+Angle     #合成一个大串   d = a+w+m+ Angle+kz
                # print("g(g):%10.3f %10.3f %10.3f a(m/s*s):%10.3f %10.3f %10.3f m():%10.3f %10.3f %10.3f angle():%10.3f %10.3f %10.3f " % d)

                # print("angle():%10.3f %10.3f %10.3f " % Angle)

                Bytenum=0
                FrameState=0
    return Angle
 
def get_gyro(datahex):   #陀螺仪
    gyro_x = (datahex[1] * 256.0 * 256.0 + datahex[2] * 256.0 + datahex[3] - 1000000) * 0.001; # gx
    gyro_y = (datahex[4] * 256.0 * 256.0 + datahex[5] * 256.0 + datahex[6] - 1000000) * 0.001; # gy
    gyro_z = (datahex[7] * 256.0 * 256.0 + datahex[8] * 256.0 + datahex[9] - 1000000) * 0.001; # gz

    return gyro_x, gyro_y, gyro_z

def get_acc(datahex):
    acc_x = (datahex[10] * 256.0 * 256.0 + datahex[11] * 256.0 + datahex[12] - 1000000) * 0.001;  # ax
    acc_y = (datahex[13] * 256.0 * 256.0 + datahex[14] * 256.0 + datahex[15] - 1000000) * 0.001;  # ay
    acc_z = (datahex[16] * 256.0 * 256.0 + datahex[17] * 256.0 + datahex[18] - 1000000) * 0.001;  # az

    return acc_x, acc_y, acc_z

def get_mg(datahex):
    mg_x = (datahex[19] * 256.0 * 256.0 + datahex[20] * 256.0 + datahex[21] - 1000000) * 0.001;  # mx
    mg_y = (datahex[22] * 256.0 * 256.0 + datahex[23] * 256.0 + datahex[24] - 1000000) * 0.001;  # my
    mg_z = (datahex[25] * 256.0 * 256.0 + datahex[26] * 256.0 + datahex[27] - 1000000) * 0.001;  # mz

    return mg_x, mg_y, mg_z
 
def get_angle(datahex):
    angle_x = (datahex[28] * 256.0 * 256.0 + datahex[29] * 256.0 + datahex[30] - 1000000) * 0.001;  # roll angle_x
    angle_y = (datahex[31] * 256.0 * 256.0 + datahex[32] * 256.0 + datahex[33] - 1000000) * 0.001;  # pitch
    angle_z = (datahex[34] * 256.0 * 256.0 + datahex[35] * 256.0 + datahex[36] - 1000000) * 0.001;  # yaw

    return angle_x,angle_y,angle_z

def get_kz(datahex):
    angle_x = (datahex[52] * 256.0 * 256.0 + datahex[53] * 256.0 + datahex[54] - 1000000) * 0.001  # 1
    angle_y = (datahex[55] * 256.0 * 256.0 + datahex[56] * 256.0 + datahex[57] - 1000000) * 0.001  #2
    angle_z = 0  #

    return angle_x,angle_y,angle_z

 
if __name__=='__main__':
    ser = serial.Serial('com4',115200, timeout=0.5)
    print("是否打开端口：", ser.is_open)
    roll = []
    pitch = []

    for i in range(1000000):
        datahex = ser.read(33)
        r = DueData(datahex)

        # time.sleep(1)
        roll.append(r[0])
        pitch.append(r[1])
        if i > 10:
            print('\rroll:%s \t pitch:%s \t yaw:%s\t rollmax:%s\t pitchmax:%s'
                  % (round(r[0], 1), round(r[1], 1), round(r[2], 1)
                     ,round(max(roll[5:])-min(roll[5:]),2),round(max(pitch[5:])-min(pitch[5:]),2)), end='')

    import time
